// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningCommon.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Motion
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Utilities common to path planning functions
    /// </summary>
    public static class PathPlanningCommon
    {
        /// <summary>
        /// Minimum drive speed (except zero) to achieve actual motion.
        /// </summary>
        public const double MinimumNonZeroTrackingSpeed = 0.01;

        /// <summary>
        /// Distance in meters below which we begin slowing down forward motion.
        /// </summary>
        public const double ArrivalSlowdownThresholdInMeters = 0.6;

        /// <summary>
        /// Distance in meters above which we consider to have strayed too far.
        /// </summary>
        public const double StrayedTooFarThresholdInMeters = 3;

        /// <summary>
        /// Maximum number of 1 neighbors.  Set to 8 to account for possible inclusion of diagonals.
        /// </summary>
        private static int maximum1Neighbors = 8;

        /// <summary>
        /// Method for indexing from a point to a map array.
        /// </summary>
        /// <param name="point">Point to index.</param>
        /// <param name="width">Width of map.</param>
        /// <param name="height">Height of map.</param>
        /// <returns>Map index corresponding to point.</returns>
        public static int PointToArrayIndex(Point2D<int> point, int width, int height)
        {
            if (point.Y < 0 || point.Y >= width)
            {
                throw new ArgumentException("point.Y");
            }

            if (point.X < 0 || point.X >= height)
            {
                throw new ArgumentException("point.X");
            }

            return (point.X * width) + point.Y;
        }

        /// <summary>
        /// Convert map array index to Point2D.
        /// </summary>
        /// <param name="index">Map index.</param>
        /// <param name="width">Map width.</param>
        /// <returns>Map point.</returns>
        public static Point2D<int> ArrayIndexToPoint(int index, int width)
        {
            return new Point2D<int>(index % width, index / width);
        }

        /// <summary>
        /// Gets valid neighbors within a Manhattan distance of 1 for indicated grid square.
        /// </summary>
        /// <param name="point">Grid square to get neighbors of.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>List of neighboring points.</returns>
        public static List<Point2D<int>> Get1Neighbors(Point2D<int> point, int width, int height)
        {
            if (point.X < 0 || point.X >= height)
            {
                throw new ArgumentException("point.X must be <=0 and <height");
            }

            if (point.Y < 0 || point.Y >= width)
            {
                throw new ArgumentException("point.Y must be <=0 and <width");
            }

            List<Point2D<int>> neighbors = new List<Point2D<int>>(maximum1Neighbors);

            if (point.X > 0)
            {
                neighbors.Add(new Point2D<int>(point.X - 1, point.Y));
            }

            if (point.X < height - 1)
            {
                neighbors.Add(new Point2D<int>(point.X + 1, point.Y));
            }

            if (point.Y > 0)
            {
                neighbors.Add(new Point2D<int>(point.X, point.Y - 1));
            }

            if (point.Y < width - 1)
            {
                neighbors.Add(new Point2D<int>(point.X, point.Y + 1));
            }

            return neighbors;
        }

        /// <summary>
        /// Gets valid neighbors within a Manhattan distance of 1 for indicated grid square.
        /// </summary>
        /// <remarks>This does *not* include diagonals.</remarks>
        /// <param name="index">Index of grid square to get neighbors of.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>Enumeration of neighboring points.</returns>
        public static IEnumerable<int> Get1NeighborIndices(int index, int width, int height)
        {
            Point2D<int> point = PathPlanningCommon.ArrayIndexToPoint(index, width);
            if (point.X > 0)
            {
                yield return index - 1;
            }

            if (point.X < width - 1)
            {
                yield return index + 1;
            }

            if (point.Y > 0)
            {
                yield return index - width;
            }

            if (point.Y < height - 1)
            {
                yield return index + width;
            }
        }

        /// <summary>
        /// Enumerate adjacent points filtered to only those within free space.
        /// </summary>
        /// <param name="i">Map index.</param>
        /// <param name="occupied">Occupancy map.</param>
        /// <param name="width">Map width.</param>
        /// <param name="height">Map height.</param>
        /// <returns>Collection of adjacent points within free space.</returns>
        public static IEnumerable<int> Get1NeighborIndicesInFreeSpace(int i, bool[] occupied, int width, int height)
        {
            return from j in PathPlanningCommon.Get1NeighborIndices(i, width, height)
                   where (j >= 0 && j < occupied.Length && !occupied[j])
                   select j;
        }

        /// <summary>
        /// Gets valid neighbors within a Manhattan distance of 1 for indicated grid square, plus all grid
        /// squares diagonally adjacent.
        /// </summary>
        /// <param name="point">Grid square to get neighbors of.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>List of neighboring points.</returns>
        public static List<Point2D<int>> Get1NeighborsWithDiagonals(Point2D<int> point, int width, int height)
        {
            List<Point2D<int>> neighbors = Get1Neighbors(point, width, height);

            if (point.X > 0)
            {
                if (point.Y > 0)
                {
                    neighbors.Add(new Point2D<int>(point.X - 1, point.Y - 1));
                }

                if (point.Y < width - 1)
                {
                    neighbors.Add(new Point2D<int>(point.X - 1, point.Y + 1));
                }
            }

            if (point.X < height - 1)
            {
                if (point.Y > 0)
                {
                    neighbors.Add(new Point2D<int>(point.X + 1, point.Y - 1));
                }

                if (point.Y < width - 1)
                {
                    neighbors.Add(new Point2D<int>(point.X + 1, point.Y + 1));
                }
            }

            return neighbors;
        }

        /// <summary>
        /// Gets valid neighbors within a Manhattan distance of 1 for indicated grid square, plus all grid
        /// squares diagonally adjacent.
        /// </summary>
        /// <param name="index">Index of grid square to get neighbors of.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>Enumeration of neighboring points.</returns>
        public static IEnumerable<int> Get1NeighborIndicesWithDiagonals(int index, int width, int height)
        {
            Point2D<int> point = PathPlanningCommon.ArrayIndexToPoint(index, width);
            bool west = point.X > 0;
            bool north = point.Y > 0;
            bool east = point.X < width - 1;
            bool south = point.Y < height - 1;
            if (west)
            {
                if (north)
                {
                    yield return index - width - 1; // NW
                }

                yield return index - 1; // W

                if (south)
                {
                    yield return index + width - 1; // SW
                }
            }

            if (north)
            {
                yield return index - width; // N
            }

            if (east)
            {
                if (north)
                {
                    yield return index - width + 1; // NE
                }

                yield return index + 1; // E

                if (south)
                {
                    yield return index + width + 1; // SE
                }
            }

            if (south)
            {
                yield return index + width; // S
            }
        }

        /// <summary>
        /// Gets the grid locations with the lowest potential from a list of locations.
        /// </summary>
        /// <param name="potentialMap">Map indicating potentials of grid squares.</param>
        /// <param name="points">A list of grid locations.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>Grid location with lowest potential.</returns>
        public static Point2D<int> GetLowestPotentialPoint(int[] potentialMap, List<Point2D<int>> points, int width, int height)
        {
            if (points == null || points.Count == 0)
            {
                throw new ArgumentException("'points' is invalid.  Must contain at least one point");
            }

            if (width * height != potentialMap.Length)
            {
                throw new ArgumentException("width and height are not consistent with map size");
            }

            int lowestPotential = int.MaxValue;

            Point2D<int> lowestPotentialPoint = default(Point2D<int>);

            int pointIndex;

            foreach (Point2D<int> p in points)
            {
                pointIndex = PointToArrayIndex(p, width, height);

                if (potentialMap[pointIndex] < lowestPotential)
                {
                    lowestPotential = potentialMap[pointIndex];
                    lowestPotentialPoint = p;
                }
            }

            return lowestPotentialPoint;
        }

        /// <summary>
        /// For a given location in the potential field, give the path that descends the 
        /// potential field along the steepest gradient and stops when a minima is found.
        /// </summary>
        /// <param name="potentialMap">The potential map.</param>
        /// <param name="currentLocation">Starting location in grid squares.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>An ordered list of points descending the gradient until a local minima is found.</returns>
        public static List<Point2D<int>> GetSteepestDescentPath(
            int[] potentialMap, Point2D<int> currentLocation, int width, int height)
        {
            List<Point2D<int>> path = new List<Point2D<int>>();

            Point2D<int> location = currentLocation;

            if (width * height != potentialMap.Length)
            {
                throw new ArgumentException("width and height are not consistent with map size");
            }

            while (true)
            {
                // check if we hit a local minima -- in which case we are done.
                if (path.Count > 0 && path[path.Count - 1] == location)
                {
                    break;
                }

                path.Add(location);

                List<Point2D<int>> neighbors = Get1NeighborsWithDiagonals(location, width, height);

                // add our current location so we stay in place if already
                // at a local minima
                neighbors.Add(location);

                location = GetLowestPotentialPoint(potentialMap, neighbors, width, height);
            }

            return path;
        }

        /// <summary>
        /// Grows the obstacles in the occupancy map by <c>growAmount</c> grid squares in every cardinal direction,
        /// </summary>
        /// <param name="occupancyMap">Map containing the probability of obstacles.  All entries must be between 0-1.</param>
        /// <param name="growAmount">Number of grid squares to grow obstacles by.</param>
        /// <param name="width">Width of map in grid squares.</param>
        /// <param name="height">Height of map in grid squares.</param>
        /// <returns>A map with the obstacles grown by the appropriate amount.</returns>
        public static bool[] GrowObstacles(bool[] occupancyMap, int growAmount, int width, int height)
        {
            if (width * height != occupancyMap.Length)
            {
                throw new ArgumentException("Width and height are not consistent with map size.");
            }

            bool[] destMap = new bool[occupancyMap.Length];
            IEnumerable<Point2D<int>> spread = CirclePoints(growAmount).ToList();

            for (int x = 0; x < height; x++)
            {
                for (int y = 0; y < width; y++)
                {
                    if (occupancyMap[x + y * width])
                    {
                        destMap[x + y * width] = true;
                        if (Get1Neighbors(new Point2D<int>(x, y), width, height).Any(z => occupancyMap[z.X + z.Y * width]))
                        {
                            foreach (Point2D<int> s in spread)
                            {
                                int i = x + s.X;
                                int j = y + s.Y;
                                if (i >= 0 && i < width && j >= 0 && j < height)
                                {
                                    destMap[i + j * width] = true;
                                }
                            }
                        }
                    }
                }
            }

            return destMap;
        }

        /// <summary>
        /// Convert pose location to map grid cell point.
        /// </summary>
        /// <param name="pose">Pose to convert.</param>
        /// <returns>Map grid cell point.</returns>
        public static Point2D<int> PosePoint(Pose2D pose)
        {
            return new Point2D<int>((int)pose.X, (int)pose.Y);
        }

        /// <summary>
        /// Euclidean distance between points.
        /// </summary>
        /// <param name="a">First point.</param>
        /// <param name="b">Second point.</param>
        /// <returns>Distance between points.</returns>
        public static double EuclideanDistance(Point2D<int> a, Point2D<int> b)
        {
            return Math.Sqrt(Math.Pow(a.Y - b.Y, 2) + Math.Pow(a.X - b.X, 2));
        }

        /// <summary>
        /// Compute Manhattan distance between two map indices.
        /// </summary>
        /// <param name="p">First map index.</param>
        /// <param name="q">Second map index.</param>
        /// <param name="width">Map width.</param>
        /// <returns>Distance between points.</returns>
        public static int ManhattanDistance(int p, int q, int width)
        {
            Point2D<int> mp = PathPlanningCommon.ArrayIndexToPoint(p, width);
            Point2D<int> mq = PathPlanningCommon.ArrayIndexToPoint(q, width);
            return Math.Abs(mp.X - mq.X) + Math.Abs(mp.Y - mq.Y);
        }

        /// <summary>
        /// Choose a target point to aim for along the path.
        /// </summary>
        /// <param name="path">Planned path of points on the occupancy map.</param>
        /// <param name="location">Location coordinate.</param>
        /// <param name="stepsAhead">Number of path points ahead to target.</param>
        /// <returns>Target point.</returns>
        public static Point2D<int> TargetPoint(IEnumerable<Point2D<int>> path, Point2D<int> location, int stepsAhead)
        {
            Point2D<int> nearestPoint = new Point2D<int>(0, 0);
            double nearestDist = double.PositiveInfinity;
            int ahead = 0;
            foreach (Point2D<int> p in path)
            {
                double dist = PathPlanningCommon.EuclideanDistance(location, p);
                if (dist < nearestDist)
                {
                    nearestDist = dist;
                    ahead = stepsAhead;
                }

                if (ahead-- >= 0)
                {
                    nearestPoint = p;
                }
            }

            return nearestPoint;
        }

        /// <summary>
        /// Find the nearest point along the path.
        /// </summary>
        /// <param name="path">Planned path of points on the occupancy map.</param>
        /// <param name="location">Location coordinate.</param>
        /// <returns>Nearest point.</returns>
        public static Point2D<int> NearestPoint(IEnumerable<Point2D<int>> path, Point2D<int> location)
        {
            return TargetPoint(path, location, 0);
        }

        /// <summary>
        /// Choose a target point to aim for along the path.
        /// </summary>
        /// <param name="target">Target point coordinate.</param>
        /// <param name="location">Location coordinate.</param>
        /// <returns>Target heading in radians.</returns>
        public static double TargetHeading(Point2D<int> target, Point2D<int> location)
        {
            return Math.Atan2((target.Y - location.Y), (target.X - location.X));
        }

        /// <summary>
        /// Keep heading within "monad" of 0 to 2PI.
        /// </summary>
        /// <param name="heading">Heading in radians.</param>
        /// <returns>Fixed heading.</returns>
        public static double FixHeadingRange(double heading)
        {
            heading %= Numerics.MathConstants.TwoPI;
            if (heading < 0.0)
            {
                heading += Numerics.MathConstants.TwoPI;
            }

            return heading;
        }

        /// <summary>
        /// Track a given path with drive directions from the given location.
        /// </summary>
        /// <param name="location">Robot location on the map.</param>
        /// <param name="plan">Path plan to track.</param>
        /// <param name="maxDistance">Maximum distance (in meters) for drive command.</param>
        /// <param name="stepsAhead">Number of map cell steps to track ahead.</param>
        /// <returns>Drive direction.</returns>
        public static DriveDirection Track(Point2D<int> location, PathPlan plan, double maxDistance, int stepsAhead)
        {
            double dist = EuclideanDistance(location, PosePoint(plan.TargetPose));
            double speed = Math.Min(1, Math.Max(MinimumNonZeroTrackingSpeed, plan.Map.MapCellsToMeters(dist) / ArrivalSlowdownThresholdInMeters));
            Point2D<int> targetPoint = PathPlanningCommon.TargetPoint(plan.Path, location, stepsAhead);
            double targetHeading = speed > 0 ? FixHeadingRange(TargetHeading(targetPoint, location)) : plan.TargetPose.Heading;
            double distance = PathPlanningCommon.EuclideanDistance(location, targetPoint) * plan.Map.DistanceResolution;
            return new DriveDirection(targetHeading, speed, Math.Min(distance, maxDistance));
        }

        /// <summary>
        /// Drive in the given direction.
        /// </summary>
        /// <remarks>With acceleration and gains for faster manual drive.</remarks>
        /// <param name="direction">Drive direction.</param>
        /// <param name="previousHeading">Last heading value.</param>
        /// <param name="currentHeading">Current heading.</param>
        /// <param name="velocity">Current velocity.</param>
        /// <param name="maxTurnRate">Maximum turn rate in radians per second.</param>
        /// <param name="maxSpeed">Maximum speed in meters per second.</param>
        /// <param name="turnGain">Speed factor proportional to turning speed (slow down/stop to turn).</param>
        /// <param name="headingDampening">Dampening factor as heading converges.</param>
        /// <param name="forwardAccelerationFactor">Factor of max speed, by which robot is allowed to accelerate.</param>
        /// <param name="forwardDecelerationFactor">Factor of max speed, by which robot is allowed to decelerate.</param>
        /// <param name="turnAccelerationFactor">Factor of max turn rate, by which robot is allowed to accelerate.</param>
        /// <param name="turnDecelerationFactor">Factor of max turn rate, by which robot is allowed to decelerate.</param>
        /// <returns>Velocity reference to be sent to the wheels.</returns>
        public static MobileBaseVelocityReference Drive(DriveDirection direction, double previousHeading, double currentHeading, MobileBaseVelocityReference velocity, double maxTurnRate, double maxSpeed, double turnGain, double headingDampening, double forwardAccelerationFactor, double forwardDecelerationFactor, double turnAccelerationFactor, double turnDecelerationFactor)
        {
            double forwardAcceleration = forwardAccelerationFactor * maxSpeed;
            double forwardDeceleration = forwardDecelerationFactor * maxSpeed;
            double turnAcceleration = turnAccelerationFactor * maxTurnRate;
            double turnDeceleration = turnDecelerationFactor * maxTurnRate;

            // turning speed
            double lastHeadingDelta = TurnTowardHeading(previousHeading, direction.Heading);
            double currentHeadingDelta = TurnTowardHeading(currentHeading, direction.Heading);
            double headingDeltaDiff = Math.Abs(currentHeadingDelta - lastHeadingDelta);
            double turn = LimitSpeedChange(
                ClampRange(currentHeadingDelta + headingDeltaDiff * headingDampening, maxTurnRate),
                velocity.ThetaInRadiansPerSecond,
                turnAcceleration,
                turnDeceleration);

            // forward speed
            double forward = LimitSpeedChange(
                ClampRange(direction.Forward, maxSpeed) * (1 - Math.Min(1, Math.Abs(currentHeadingDelta * turnGain))),
                velocity.XInMetersPerSecond,
                forwardAcceleration,
                forwardDeceleration);

            return new MobileBaseVelocityReference(forward, 0, turn);
        }

        /// <summary>
        /// Determine whether planned path is obstructed on the map.
        /// </summary>
        /// <param name="map">Current (presumably dynamically updated) global map.</param>
        /// <param name="location">Current robot location on the map.</param>
        /// <param name="plan">Current planned path.</param>
        /// <param name="radiusInMapCells">Robot radius in map cells (may be padded to account for deviation, etc.).</param>
        /// <param name="minObstacleSizeInMapCells">Minimum obstacle size in map cells.</param>
        /// <param name="maxObstacleDistanceInMapCells">Maximum obstacle distance in map cells beyond which we don't consider.</param>
        /// <returns>Whether obstructed.</returns>
        public static bool Obstructed(
            GlobalMap map,
            Point2D<int> location,
            IEnumerable<Point2D<int>> plan,
            int radiusInMapCells,
            int minObstacleSizeInMapCells,
            int maxObstacleDistanceInMapCells)
        {
            Point2D<int> nearest = NearestPoint(plan, location);
            IEnumerable<Point2D<int>> collisionPoints = RelativeCollisionPoints(radiusInMapCells, minObstacleSizeInMapCells);

            return
                map != null && plan != null &&
                plan.SkipWhile(p => p != nearest) // starting from where we are
                    .Take(maxObstacleDistanceInMapCells) // look forward n-cells
                    .SelectMany(p => collisionPoints.Select(c => new Point2D<int>(c.X + p.X, c.Y + p.Y))) // along points sampled around the robot
                    .Where(p => p.X >= 0 && p.X < map.Height && p.Y >= 0 && p.Y < map.Width) // that are not off-map
                    .Any(p => !map.IsFree(p.X, p.Y)); // occupied?
        }

        /// <summary>
        /// Slow down as distance traveled approaches the last requested drive distance.
        /// </summary>
        /// <param name="drive">Current drive direction.</param>
        /// <param name="translationDistanceTraveled">Distance travelled in meters.</param>
        /// <returns>Adjusted drive direction.</returns>
        public static double SpeedFactorApproachingTarget(
            DriveDirection drive,
            double translationDistanceTraveled)
        {
            if (drive.Distance != 0)
            {
                return 1 - Math.Min(1, translationDistanceTraveled / drive.Distance);
            }

            return 1;
        }

        /// <summary>
        /// Slow down as distance traveled approaches the last requested drive distance.
        /// </summary>
        /// <param name="drive">Current drive direction.</param>
        /// <param name="factor">Forward speed limiting factor.</param>
        /// <returns>Adjusted drive direction.</returns>
        public static DriveDirection LimitForwardSpeed(DriveDirection drive, double factor)
        {
            double forward = drive.Forward * factor;
            if (forward > 0)
            {
                forward = Math.Max(MinimumNonZeroTrackingSpeed, forward);
            }

            return new DriveDirection(drive.Heading, forward, drive.Distance);
        }

        /// <summary>
        /// Convert pose relative to map origin (in meters) to pose relative to start pose in map cells.
        /// </summary>
        /// <param name="map">Global map.</param>
        /// <param name="pose">Pose relative to map origin.</param>
        /// <returns>Pose relative to start.</returns>
        public static Pose2D MapPoseInMetersToStartPoseRelativeInMapCells(GlobalMap map, Pose2D pose)
        {
            return new Pose2D(map.MetersToMapCells(pose.X) + map.MapStartPose.X, map.MetersToMapCells(pose.Y) + map.MapStartPose.Y, pose.Heading + map.MapStartPose.Heading);
        }

        /// <summary>
        /// Determine whether we've strayed too far from the path.
        /// </summary>
        /// <param name="plan">Planned path.</param>
        /// <param name="location">Current location.</param>
        /// <param name="metersToMapCellsFn">Map resolution specific function to convert from meters to map cells.</param>
        /// <returns>Whether we've strayed too far.</returns>
        public static bool StrayedTooFar(IEnumerable<Point2D<int>> plan, Point2D<int> location, Func<double, double> metersToMapCellsFn)
        {
            Point2D<int> target = TargetPoint(plan, location, 0);
            double distance = EuclideanDistance(target, location);
            double tooFar = metersToMapCellsFn(StrayedTooFarThresholdInMeters);
            return distance > tooFar;
        }

        /// <summary>
        /// Generate points used to sample for collisions with the robot.
        /// </summary>
        /// <param name="radiusInMapCells">Robot radius in map cells (may be padded to account for deviation, etc.).</param>
        /// <param name="minObstacleSizeInMapCells">Minimum obstacle size in map cells.</param>
        /// <returns>Collision points.</returns>
        private static IEnumerable<Point2D<int>> RelativeCollisionPoints(int radiusInMapCells, int minObstacleSizeInMapCells)
        {
            double r = 2 * radiusInMapCells * radiusInMapCells;
            double a = Math.Acos((r - minObstacleSizeInMapCells) / r);
            List<Point2D<int>> points = new List<Point2D<int>>();
            for (double i = 0; i < 2 * Math.PI; i += a)
            {
                points.Add(new Point2D<int>(
                    (int)(Math.Cos(i) * radiusInMapCells + 0.5),
                    (int)(Math.Sin(i) * radiusInMapCells + 0.5)));
            }

            return points;
        }

        /// <summary>
        /// Clamp value with +/- range.
        /// </summary>
        /// <param name="value">Value to clamp.</param>
        /// <param name="range">Range to which to clamp.</param>
        /// <returns>Clamped value.</returns>
        private static double ClampRange(double value, double range)
        {
            return Math.Min(range, Math.Max(-range, value));
        }

        /// <summary>
        /// Turn in the shortest direction toward the given heading.
        /// </summary>
        /// <param name="heading">Current heading.</param>
        /// <param name="target">Target heading.</param>
        /// <returns>Heading to target.</returns>
        private static double TurnTowardHeading(double heading, double target)
        {
            if (double.IsNaN(target))
            {
                return 0; // target is current heading by definition
            }

            double turn = target - heading;
            return turn + MathConstants.TwoPI * (turn > Math.PI ? -1 : turn < -Math.PI ? 1 : 0);
        }

        /// <summary>
        /// Limit speed changes.
        /// </summary>
        /// <param name="proposed">Proposed new speed.</param>
        /// <param name="current">Current speed.</param>
        /// <param name="maxPositiveChange">Maximum positive change in speed.</param>
        /// <param name="maxNegativeChange">Maximum negative change in speed.</param>
        /// <returns>Limited speed value (or proposed value if within limits).</returns>
        private static double LimitSpeedChange(double proposed, double current, double maxPositiveChange, double maxNegativeChange)
        {
            if (double.IsNaN(proposed))
            {
                return current;
            }

            double change = proposed - current;
            double absChange = Math.Abs(change);
            int sign = Math.Sign(change);

            if ((sign == -1 && change < maxNegativeChange) ||
                (sign == 1 && change > -maxNegativeChange))
            {
                // decelerating too fast
                return current - sign * maxNegativeChange;
            }

            if (absChange > maxPositiveChange)
            {
                // accelerating too fast
                return current + sign * maxPositiveChange;
            }

            return proposed;
        }

        /// <summary>
        /// Circle points (used for euclidean distance threshold obstacle growth).
        /// </summary>
        /// <param name="radius">Circle radius.</param>
        /// <returns>Points around circle.</returns>
        private static IEnumerable<Point2D<int>> CirclePoints(int radius)
        {
            int x = radius;
            int y = 0;
            int error = 1 - x;

            while (x >= y)
            {
                yield return new Point2D<int>(x, y);
                yield return new Point2D<int>(y, x);
                yield return new Point2D<int>(-x, y);
                yield return new Point2D<int>(-y, x);
                yield return new Point2D<int>(x, -y);
                yield return new Point2D<int>(y, -x);
                yield return new Point2D<int>(-x, -y);
                yield return new Point2D<int>(-y, -x);

                y++;
                if (error < 0)
                {
                    error += 2 * y + 1;
                }
                else
                {
                    x--;
                    error += 2 * (y - x + 1);
                }
            }
        }
    }
}
